﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using teleporter = Microsoft.Robotics.Services.Simulation.Teleporter.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using System.Windows;

namespace ForceFields
{
    [Contract(Contract.Identifier)]
    [DisplayName("ForceFields")]
    [Description("ForceFields service (no description provided)")]
    class ForceFieldsService : DsspServiceBase
    {
        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        ForceFieldsState _state = new ForceFieldsState();
        Port<DateTime> _dateTimePort = new Port<DateTime>();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/ForceFields", AllowMultipleInstances = true)]
        ForceFieldsOperations _mainPort = new ForceFieldsOperations();

        [SubscriptionManagerPartner]
        submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// TeleporterServiceP3DX partner
        /// </summary>
        [Partner("TeleporterServiceP3DX", Contract = teleporter.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        teleporter.TeleporterOperations _teleporterServiceP3DXPort = new teleporter.TeleporterOperations();
        teleporter.TeleporterOperations _teleporterServiceP3DXNotify = new teleporter.TeleporterOperations();
        private teleporter.TeleporterOperations _teleportPort5 = new teleporter.TeleporterOperations();

        /// <summary>
        /// TeleporterServiceStreetCone1 partner
        /// </summary>
        [Partner("TeleporterServiceStreetCone1", Contract = teleporter.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        teleporter.TeleporterOperations _teleporterServiceStreetCone1Port = new teleporter.TeleporterOperations();
        teleporter.TeleporterOperations _teleporterServiceStreetCone1Notify = new teleporter.TeleporterOperations();

        /// <summary>
        /// TeleporterServiceStreetCone2 partner
        /// </summary>
        [Partner("TeleporterServiceStreetCone2", Contract = teleporter.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        teleporter.TeleporterOperations _teleporterServiceStreetCone2Port = new teleporter.TeleporterOperations();
        teleporter.TeleporterOperations _teleporterServiceStreetCone2Notify = new teleporter.TeleporterOperations();

        /// <summary>
        /// TeleporterServiceGreyBox partner
        /// </summary>
        [Partner("TeleporterServiceGreyBox", Contract = teleporter.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        teleporter.TeleporterOperations _teleporterServiceGreyBoxPort = new teleporter.TeleporterOperations();
        teleporter.TeleporterOperations _teleporterServiceGreyBoxNotify = new teleporter.TeleporterOperations();

        /// <summary>
        /// DriveDifferentialTwoWheel partner
        /// </summary>
        [Partner("DriveDifferentialTwoWheel", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        drive.DriveOperations _driveDifferentialTwoWheelPort = new drive.DriveOperations();
        drive.DriveOperations _driveDifferentialTwoWheelNotify = new drive.DriveOperations();

        #region Parameters
        private static int ms = 300;
        #endregion

        /// <summary>
        /// Service constructor
        /// </summary>
        public ForceFieldsService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        /// <summary>
        /// Service start
        /// </summary>
        protected override void Start()
        {

            // Here is where we send a message to the _dateTimePort so we can poll all the sensors again using the
            // UpdateSensorData method after waiting 1000 millisecond.
            Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateSensorData));
            TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(ms), _dateTimePort);

            base.Start();
        }

        IEnumerator<ITask> UpdateSensorData(DateTime dateTime)
        {
            getData();

            Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateSensorData));
            TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(ms), _dateTimePort);
            yield break;
        }

        private void getData()
        {
            // Get the robot's position
            teleporter.TeleporterState sensorStateP3DX = getState(_teleporterServiceP3DXPort);
            // Get the goal's position
            teleporter.TeleporterState sensorStateGreyBox = getState(_teleporterServiceGreyBoxPort);

            // Get the obstacles positions
            teleporter.TeleporterState sensorStateStreetCone1 = getState(_teleporterServiceStreetCone1Port);
            teleporter.TeleporterState sensorStateStreetCone2 = getState(_teleporterServiceStreetCone2Port);

            if (sensorStateP3DX != null)
            {
                Console.WriteLine("GPS data: X:{0} Z:{1}", sensorStateP3DX.Position.X, sensorStateP3DX.Position.Z);
                _state.coordsP3DX.X = sensorStateP3DX.Position.X;
                _state.coordsP3DX.Y = sensorStateP3DX.Position.Z;
            }

            if (sensorStateGreyBox != null)
            {
                Console.WriteLine("GPS data: X:{0} Z:{1}", sensorStateGreyBox.Position.X, sensorStateGreyBox.Position.Z);
            }

            if ((sensorStateStreetCone1 != null) && (sensorStateStreetCone2 != null))
            {
                Console.WriteLine("GPS data: X:{0} Z:{1}", sensorStateStreetCone1.Position.X, sensorStateStreetCone1.Position.Z);
                Console.WriteLine("GPS data: X:{0} Z:{1}", sensorStateStreetCone2.Position.X, sensorStateStreetCone2.Position.Z);
            }
        }

        private void calculatePath()
        {
            teleporter.TeleporterState sensorState = getState(_teleporterServiceP3DXPort);
            if (sensorState != null)
            {
                Console.WriteLine("GPS data: X:{0} Y:{1}", sensorState.Position.X, sensorState.Position.Y, sensorState.Position.Z);
                _state.coordsP3DX.X = sensorState.Position.X;
                _state.coordsP3DX.Y = sensorState.Position.Z;

                Vector goal = new Vector(-20.0, -15.0);

                // You can use this code to send commands to the differential drive to power your left and right wheels of your vehicle
                //drive.SetDrivePowerRequest setPower = new drive.SetDrivePowerRequest(.4f, .4f);
                //_driveDifferentialTwoWheelPort.SetDrivePower(setPower);

                double newDegrees = Vector.AngleBetween(_state.coordsP3DX, goal) - _state.degrees;

                _state.degrees = (_state.degrees + newDegrees) % 360;
                Console.WriteLine("Degrees: {0}", _state.degrees);
                drive.RotateDegreesRequest rotateDegrees = new drive.RotateDegreesRequest(newDegrees, .4f);
                _driveDifferentialTwoWheelPort.RotateDegrees(rotateDegrees);
            }
        }

        private teleporter.TeleporterState getState(teleporter.TeleporterOperations telePort)
        {
            var sensorOrFault = telePort.Get();

            Console.WriteLine(">>> reading port connection " + telePort);
            teleporter.TeleporterState sensorState = (teleporter.TeleporterState)sensorOrFault;
            if (sensorState != null)
            {
                Console.WriteLine(">>> NO ERROR");
                return sensorState;
            }
            else
            {
                Console.WriteLine(">>> ERROR");
                return null;
            }
        }

        /// <summary>
        /// Handles Subscribe messages
        /// </summary>
        /// <param name="subscribe">the subscribe request</param>
        [ServiceHandler]
        public void SubscribeHandler(Subscribe subscribe)
        {
            SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
        }
    }
}


